#include "safety_recheck.h"
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#include <math.h>
#include <sys/time.h>
#include <string.h>
#include <time.h>
#include "safety_recheck_itf.h"
#include "errcode.h"
#include "dyna/jaka_dyn_intf.h"
#include "zuc/nml_intf/zucpos.h"
#include "nml_intf/zucpose.h"
#include "motion/semintf.h"
#include "motion/motionStrategyType.h"
#include "motion/controlDefine.h"
#include "axisgroup/collision_detection.h"
//#include "tp/tpmanager.hh"
#include "remoteApi/usrRemote.h"
#include "motion/IOmapping.h"
#include "AttitudeLimit.h"
#include "motion/timemonitor.h"
#include "axisgroup/axisgroup.h"
#include "axisgroup/axisgroup_robot.h"
#include "axisgroup/robot_mgr.h"
#include "kine/kine.h"

extern zucmot_status_t* zucmotStatus;
extern zucmot_debug_t* zucmotDebug;
extern zucmot_config_t* zucmotConfig;
extern zucmot_joint_t* joints;
extern zucmot_axis_t* axes;

static safety_info_t safety_info = {0};

static int kinematicsInverse(const ZucPose* toolOffset,
                             const ZucPose* world,
                             double* joint,
                             const unsigned long* interp_iflags,
                             unsigned long* interp_fflags,
                             const PmRpy* base_offset,
                             const ZucPose* user_offset,
                             uint8_t grp_id = 0)
{
    RobJointVal jpos(joint);
    kine::KineItf::kine_Conf conf;
    int ret = axisgroup::AxisGroupMgr::instance().get_axisgroup(grp_id)->kine()->ikine(*world, jpos, conf, toolOffset, user_offset, base_offset);
    jpos.array_value(joint);
    return ret;
}
static int kinematicsForward(const ZucPose* toolOffset,
                             const double* joint,
                             ZucPose* world,
                             const unsigned long* interp_fflags,
                             unsigned long* interp_iflags,
                             const PmRpy* base_offset,
                             const ZucPose* user_offset,
                             uint8_t grp_id = 0)
{
    int ret = -1;
    RobJointVal jpos(joint);
    ZucPose pose;
    ret = axisgroup::AxisGroupMgr::instance().get_axisgroup(grp_id)->kine()->fkine(jpos, pose, toolOffset, user_offset, base_offset);
    *world = pose;
    return ret;
}
void safety_func_check()
{
    if (zucmotStatus->motion_state == ZUCMOT_MOTION_DISABLED || zucmotStatus->motion_state == ZUCMOT_MOTION_HOME ||
        zucmotStatus->motion_state == ZUCMOT_MOTION_ADMITTANCE || zucmotStatus->motion_state == ZUCMOT_MOTION_SERVOJOP /* || ZUCMOT_MOTION_DRAG*/)
    {
        // 未使能、伺服回零、力控拖拽、无力控拖拽: update and return
        for (int jnum = 0; jnum < ZUCMOT_MAX_JOINTS; jnum++)
        {
            safety_info.last_last_jpos_fb[jnum] = safety_info.last_jpos_fb[jnum];
            safety_info.last_jpos_fb[jnum] = joints[jnum].pos_fb;
        }
        // safety_info.last_elbow_pos_fb = safety_info.elbow_pos_fb;
        // safety_info.last_carte_pos_fb = safety_info.carte_pos_fb;
        return;
    }

    safety_double_check_inputp_t input_status = {0};
    input_status.motion_state = zucmotStatus->motion_state;
    for (int jnum = 0; jnum < ZUCMOT_MAX_JOINTS; jnum++)
    {
        /*
        需确认是以反馈关节位置为输入还是指令关节位置为输入;
        若以反馈关节角度为输入，需确认： 
        1. 目前关节位置、速度以及线速度设置的容许阈值是否合适； 
        2. 安全平面以及工具姿态约束的阈值应设置多少比较合适(机器人运动学模型存在误差会导致误判的发生, 目前未设置误差阈值);
        */
        input_status.current_jpos_fb[jnum] = joints[jnum].pos_fb;
    }
    input_status.tool_offset = zucmotConfig->tool_offset;
    input_status.base_offset = zucmotConfig->base_offset;
    input_status.user_offset = zucmotConfig->user_offset;
    input_status.trajCycleTime = zucmotConfig->trajCycleTime;
    input_status.dragging = zucmotDebug->dragging;                  //是否退出拖拽
    input_status.not_trig_abort = is_tpabort_motabort_triggered();  //是否触发tpAbort和motabortmotion
    input_status.reduce_mode = zucmotStatus->reduce_mode;           //是否触发tpAbort和motabortmotion
    //    input_status.tp_is_pausing = tpManager_tpIsPausing();           //是否触发暂停程序
    input_status.dec_dist = 0.0;        //减速到零所需距离
    input_status.dec_time = 0.0;        //减速到零所需时间
    input_status.now_not_run_prog = 0;  //是否是执行程序时的联动模式, 0: 执行程序时的联动模式; 1: 关节/直线运动至目标位置
    // if (input_status.motion_state == ZUCMOT_MOTION_COORD)
    // {
    //     //        int is_null_tc1 = tpManager_GetStopDist(0, &input_status.dec_dist);
    //     double safety_acc = 0.0;
    //     double currentVel = 0.0;
    //     //        int get_acc_ret = tpManager_GetSafetyAcc(0, &safety_acc);
    //     //        int get_vel_ret = tpManager_GetCurPlanVel(0, &currentVel);  //获取规划主轴的current规划速度
    //     if (safety_acc > Q_FUZZ)
    //     {
    //         input_status.dec_time = fabs(currentVel / safety_acc);
    //     }
    //     unsigned char coord_mot_enables = 1;
    //     //        int is_null_tc2 = tpManager_GetMotEnables(0, &coord_mot_enables);
    //     if (!is_null_tc2 && !coord_mot_enables)
    //     {
    //         input_status.now_not_run_prog = 1;
    //     }
    //     if (is_null_tc1 != 0 || get_acc_ret != 0 || get_vel_ret != 0 || is_null_tc2 != 0)
    //     {
    //         // rtapi_print(" ============>>>>>>>>>>>> tp is empty when coordinate, copy pos and return directly\n");
    //         for (int jnum = 0; jnum < ZUCMOT_MAX_JOINTS; jnum++)
    //         {
    //             safety_info.last_last_jpos_fb[jnum] = safety_info.last_jpos_fb[jnum];
    //             safety_info.last_jpos_fb[jnum] = joints[jnum].pos_fb;
    //         }
    //         return;
    //     }
    // }

    int double_check_ret = 0;
    double_check_ret = safety_double_check(&input_status);

    //update last_jpos_fb, last_cart_pos_fb, last_elbow_pos_fb
    memcpy(safety_info.last_last_jpos_fb, safety_info.last_jpos_fb, ZUCMOT_MAX_JOINTS * sizeof(double));
    memcpy(safety_info.last_jpos_fb, input_status.current_jpos_fb, ZUCMOT_MAX_JOINTS * sizeof(double));
    // safety_info.last_elbow_pos_fb = safety_info.elbow_pos_fb;
    // safety_info.last_carte_pos_fb = safety_info.carte_pos_fb;

    if (double_check_ret)
    {
        trigger_abort();
    }
    return;
}

int safety_double_check(safety_double_check_inputp_t* input_status)
{
    int jpos_violation = 0;
    int jvel_violation = 0;
    int tcp_vel_violation = 0;
    int elbow_vel_violation = 0;
    safe_plane_violation_t safeplane_violate = {0};
    int tool_ori_violation = 0;
    int stop_time_violation = 0;
    int stop_dist_violation = 0;

    double current_jvel[ZUCMOT_MAX_JOINTS] = {0.0};
    for (int jnum = 0; jnum < ZUCMOT_MAX_JOINTS; jnum++)
    {
        current_jvel[jnum] = (input_status->current_jpos_fb[jnum] - safety_info.last_jpos_fb[jnum]) / input_status->trajCycleTime;
    }

    KINEMATICS_FORWARD_FLAGS safecheck_fflags = 0;
    KINEMATICS_INVERSE_FLAGS safecheck_iflags = 0;
    ZucPose last_carte_pos_fb = {0};
    ZucPose last_last_carte_pos_fb = {0};
    ZucPose carte_pos_fb = {0};
    ZucPose elbow_pos_fb = {0};
    ZucPose last_elbow_pos_fb = {0};
    ZucPose last_last_elbow_pos_fb = {0};

    //kinematics forward to get tcp and elbow pos
    kinematicsForward(&input_status->tool_offset,
                      input_status->current_jpos_fb,
                      &carte_pos_fb,
                      &safecheck_fflags,
                      &safecheck_iflags,
                      &input_status->base_offset,
                      &input_status->user_offset);
    // kinematicsForward_elbow(input_status->current_jpos_fb, &elbow_pos_fb, &input_status->base_offset, &input_status->user_offset);

    kinematicsForward(&input_status->tool_offset,
                      safety_info.last_jpos_fb,
                      &last_carte_pos_fb,
                      &safecheck_fflags,
                      &safecheck_iflags,
                      &input_status->base_offset,
                      &input_status->user_offset);
    // kinematicsForward_elbow(safety_info.last_jpos_fb, &last_elbow_pos_fb, &input_status->base_offset, &input_status->user_offset);

    kinematicsForward(&input_status->tool_offset,
                      safety_info.last_last_jpos_fb,
                      &last_last_carte_pos_fb,
                      &safecheck_fflags,
                      &safecheck_iflags,
                      &input_status->base_offset,
                      &input_status->user_offset);
    // kinematicsForward_elbow(safety_info.last_last_jpos_fb, &last_last_elbow_pos_fb, &input_status->base_offset, &input_status->user_offset);

    // get pos related to {world} for re-checking safety plane
    ZucPose cart_pos_fb_to_world = {0};
    ZucPose last_cart_pos_fb_to_world = {0};
    ZucPose elbow_pos_fb_to_world = {0};
    ZucPose last_elbow_pos_fb_to_world = {0};
    convertPointToWorld(&carte_pos_fb, &input_status->user_offset, &cart_pos_fb_to_world);
    convertPointToWorld(&last_carte_pos_fb, &input_status->user_offset, &last_cart_pos_fb_to_world);
    convertPointToWorld(&elbow_pos_fb, &input_status->user_offset, &elbow_pos_fb_to_world);
    convertPointToWorld(&last_elbow_pos_fb, &input_status->user_offset, &last_elbow_pos_fb_to_world);

    //switch motion_state
    switch (input_status->motion_state)
    {
    case ZUCMOT_MOTION_FREE:
    case ZUCMOT_MOTION_TELEOP:
    case ZUCMOT_MOTION_TELEOP_TOOL: {
        //check joint pos limit
        jpos_violation =
            safety_check_jpos_limit(input_status->current_jpos_fb, safety_info.safety_config.max_pos_limit, safety_info.safety_config.min_pos_limit);

        //check joint vel limit
        jvel_violation = safety_check_jvel_limit(current_jvel, safety_info.safety_config.jvel_limit);

        //check TCP vel limit
        tcp_vel_violation = safety_check_tcp_vel_limit(&carte_pos_fb, &last_carte_pos_fb, &last_last_carte_pos_fb, input_status);

        //安全平面是否设置“开机启动”
        int safe_plane_open_enable = safety_info.safety_config.zone.openEnable;

        if (safe_plane_open_enable)
        {
            safeplane_violate =
                safety_check_safe_plane(&cart_pos_fb_to_world, &last_cart_pos_fb_to_world, &elbow_pos_fb, &last_elbow_pos_fb_to_world, input_status);
        }

        //工具姿态约束是否设置“开机启动”
        int tool_ori_open_enable = safety_info.safety_config.tool_ori.enableLimAttitudeSafetyOpen;
        tool_ori_violation = 0;
        if (tool_ori_open_enable)
        {
            tool_ori_violation =
                safety_check_tool_ori(&cart_pos_fb_to_world, &last_cart_pos_fb_to_world, &input_status->tool_offset, input_status->not_trig_abort);
        }
        break;
    }

    case ZUCMOT_MOTION_COORD: {
        //check joint pos limit
        jpos_violation =
            safety_check_jpos_limit(input_status->current_jpos_fb, safety_info.safety_config.max_pos_limit, safety_info.safety_config.min_pos_limit);

        //check joint vel limit
        jvel_violation = safety_check_jvel_limit(current_jvel, safety_info.safety_config.jvel_limit);

        //check TCP vel limit
        tcp_vel_violation = safety_check_tcp_vel_limit(&carte_pos_fb, &last_carte_pos_fb, &last_last_carte_pos_fb, input_status);

        //check elbow vel limit
        elbow_vel_violation = safety_check_elbow_vel_limit(&elbow_pos_fb, &last_elbow_pos_fb, &last_last_elbow_pos_fb, input_status);

        //check stop time limit
        stop_time_violation = safety_check_stop_time_limit(input_status);

        //check stop dist limit
        stop_dist_violation = safety_check_stop_dist_limit(input_status);

        //安全平面是否设置“开机启动”或"运行启用",设置"运行启用"时不对手动操作直线/关节运动至该点生效
        int safe_plane_open_enable = 0;
        if (input_status->now_not_run_prog)
        {
            safe_plane_open_enable = safety_info.safety_config.zone.openEnable;
        }
        else
        {
            safe_plane_open_enable = safety_info.safety_config.zone.openEnable || safety_info.safety_config.zone.autoEnable;
        }

        if (safe_plane_open_enable)
        {
            safeplane_violate =
                safety_check_safe_plane(&cart_pos_fb_to_world, &last_cart_pos_fb_to_world, &elbow_pos_fb, &last_elbow_pos_fb_to_world, input_status);
        }

        //工具姿态约束是否设置“开机启动”或"运行启用",设置"运行启用"时不对手动操作直线/关节运动至该点生效
        int tool_ori_open_enable = 0;
        if (input_status->now_not_run_prog)
        {
            tool_ori_open_enable = safety_info.safety_config.tool_ori.enableLimAttitudeSafetyOpen;
        }
        else
        {
            tool_ori_open_enable =
                safety_info.safety_config.tool_ori.enableLimAttitudeSafetyOpen || safety_info.safety_config.tool_ori.enableLimAttitudeSafetyAuto;
        }
        if (tool_ori_open_enable)
        {
            tool_ori_violation =
                safety_check_tool_ori(&cart_pos_fb_to_world, &last_cart_pos_fb_to_world, &input_status->tool_offset, input_status->not_trig_abort);
        }
        break;
    }
    case ZUCMOT_MOTION_DRAG: {
        //做安全平面检测 and tool oritation constrain
        int safe_plane_open_enable = safety_info.safety_config.zone.openEnable && input_status->dragging;
        if (safe_plane_open_enable)
        {
            safeplane_violate =
                safety_check_safe_plane(&cart_pos_fb_to_world, &last_cart_pos_fb_to_world, &elbow_pos_fb, &last_elbow_pos_fb_to_world, input_status);
        }

        int tool_ori_open_enable = safety_info.safety_config.tool_ori.enableLimAttitudeSafetyOpen && input_status->dragging;
        if (tool_ori_open_enable)
        {
            tool_ori_violation =
                safety_check_tool_ori(&cart_pos_fb_to_world, &last_cart_pos_fb_to_world, &input_status->tool_offset, input_status->not_trig_abort);
        }

        break;
    }
    default:
        break;
    }
    if (jpos_violation || jvel_violation || tcp_vel_violation || elbow_vel_violation || safeplane_violate.safe_plane_violation || tool_ori_violation ||
        stop_time_violation || stop_dist_violation)
    {
        rtapi_print("jpos_violation = %d, jvel_violation = %d, tcp_vel_violation = %d, elbow_vel_violation = %d, safe_plane_violation = %d, tool_ori_violation "
                    "= %d, stop_time_violation = %d, stop_dist_violation = %d\n",
                    jpos_violation,
                    jvel_violation,
                    tcp_vel_violation,
                    elbow_vel_violation,
                    safeplane_violate.safe_plane_violation,
                    tool_ori_violation,
                    stop_time_violation,
                    stop_dist_violation);
        return 1;
    }

    return 0;
}

void trigger_abort()
{
    //tpAbort motmotionabort
    // SET_MOTION_ERROR_FLAG(1);
    //    tpManager_tpabort();
    int axis_num = 0;
    int joint_num = 0;
    zucmot_joint_t* joint;
    zucmot_axis_t* axis;

    /* zucMotionAbort(); */
    // for (axis_num = 0; axis_num < CART_DIM; axis_num++)
    // {
    //     axis = &axes[axis_num];
    //     axis->teleop_tp.enable = 0;
    //     axis->teleop_tool_tp.enable = 0;
    // }

    // for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    // {
    //     joint = &joints[joint_num];
    //     joint->free_tp.enable = 0;
    // }

    // zucmotDebug->dragging = 0;
    // if (GET_MOTION_TELEOP_FLAG())
    // {
    //     for (axis_num = 0; axis_num < CART_DIM; axis_num++)
    //     {
    //         axes[axis_num].teleop_tp.enable = 0;
    //         axes[axis_num].teleop_tp.max_acc = (axis_num < 3) ? 3000.0 : 1080.0;
    //     }
    // }
    // else if (GET_MOTION_TELEOP_TOOL_FLAG())
    // {
    //     for (axis_num = 0; axis_num < CART_DIM; axis_num++)
    //     {
    //         axes[axis_num].teleop_tool_tp.enable = 0;
    //         axes[axis_num].teleop_tool_tp.max_acc = (axis_num < 3) ? 3000.0 : 1080.0;
    //     }
    // }

    if (zucmotStatus->identifying_status == 1)
    {
        zucmotStatus->identifying_status = -1;
    }
    return;
}

/*
judge jpos, return 1: jpos outof limit
*/
int safety_check_jpos_limit(double* current_jpos_fb, double* max_jpos_limit, double* min_jpos_limit)
{
    // 根据当前机型关节轴数进行判断，避免多余轴内存被影响导致误判
    int ret = 0;
    for (int jnum = 0; jnum < safety_info.numJoints; jnum++)
    {
        int temp_ret = check_jpos_limit(current_jpos_fb[jnum], max_jpos_limit[jnum] + JOINT_POS_THRESHOLD, min_jpos_limit[jnum] - JOINT_POS_THRESHOLD);
        if (temp_ret)
        {
            // rtapi_print("[safety_check_jpos_limit] jnum = %d, current_jpos_fb = %f, max_jpos_limit = %f, min_jpos_limit = %f, JOINT_POS_THRESHOLD = %f \n",
            //             jnum,
            //             current_jpos_fb,
            //             max_jpos_limit,
            //             min_jpos_limit,
            //             JOINT_POS_THRESHOLD);
        }
        ret = ret || temp_ret;
    }
    return ret;
}

/*
judge jvel, return 1: jvel outof limit
*/
int safety_check_jvel_limit(double* current_jvel, double* max_jvel_limit)
{
    // 根据当前机型关节轴数进行判断，避免多余轴内存被影响导致误判
    int ret = 0;
    for (int jnum = 0; jnum < safety_info.numJoints; jnum++)
    {
        int temp_ret = check_jvel_limit(current_jvel[jnum], max_jvel_limit[jnum] + JOINT_SPEED_THRESHOLD);
        if (temp_ret)
        {
            rtapi_print("[safety_check_jvel_limit] %dth : current_jvel = %f, max_jvel_limit = %f, JOINT_SPEED_THRESHOLD = %f\n",
                        jnum,
                        current_jvel[jnum],
                        max_jvel_limit[jnum],
                        JOINT_SPEED_THRESHOLD);
        }
        ret = ret || temp_ret;
    }
    return ret;
}

/*
judge tcp vel, return 1 outof limit
需要判断机器人是否处于缩减模式,根据模式取约束值
*/
int safety_check_tcp_vel_limit(ZucPose* current_cart_pos, ZucPose* last_cart_pos, ZucPose* last_last_cart_pos, safety_double_check_inputp_t* input_status)
{
    double dx = current_cart_pos->tran.x - last_cart_pos->tran.x;
    double dy = current_cart_pos->tran.y - last_cart_pos->tran.y;
    double dz = current_cart_pos->tran.z - last_cart_pos->tran.z;
    double tcp_vel = sqrt(dx * dx + dy * dy + dz * dz) / input_status->trajCycleTime;

    dx = last_last_cart_pos->tran.x - last_cart_pos->tran.x;
    dy = last_last_cart_pos->tran.y - last_cart_pos->tran.y;
    dz = last_last_cart_pos->tran.z - last_cart_pos->tran.z;
    double last_tcp_vel = sqrt(dx * dx + dy * dy + dz * dz) / input_status->trajCycleTime;

    double tcp_vel_limit = safety_info.safety_config.tcpVelLimit;
    //reduce mode?
    int is_reduce_mode = input_status->reduce_mode;
    if (is_reduce_mode)
    {
        tcp_vel_limit = tcp_vel_limit > safety_info.safety_config.tcp_vel_reduce;
    }

    tcp_vel_limit += LINEAR_SPEED_THRESHOLD;
    //若已超速度限制且未开始减速，则异常
    int overspeed = check_linear_vel_limit(tcp_vel, tcp_vel_limit);

    if (overspeed && tcp_vel > last_tcp_vel)
    {
        rtapi_print("[safety_check_tcp_vel_limit] : tcp_vel = %f, tcp_vel_limit = %f, last_tcp_vel = %f\n", tcp_vel, tcp_vel_limit, last_tcp_vel);
        return 1;
    }

    return 0;
}

/*
judge elbow vel, return 1 outof limit
需要判断机器人是否处于缩减模式,根据模式取约束值
TODO: 若后续1.7.2增加非缩减模式下的肘部速度约束，此处需修改
*/
int safety_check_elbow_vel_limit(ZucPose* current_elbow_pos, ZucPose* last_elbow_pos, ZucPose* last_last_elbow_pos, safety_double_check_inputp_t* input_status)
{
    //reduce mode?
    int is_reduce_mode = input_status->reduce_mode;
    if (!is_reduce_mode)
    {
        return 0;
    }

    double dx = current_elbow_pos->tran.x - last_elbow_pos->tran.x;
    double dy = current_elbow_pos->tran.y - last_elbow_pos->tran.y;
    double dz = current_elbow_pos->tran.z - last_elbow_pos->tran.z;
    double elbow_vel = sqrt(dx * dx + dy * dy + dz * dz) / input_status->trajCycleTime;

    dx = last_elbow_pos->tran.x - last_last_elbow_pos->tran.x;
    dy = last_elbow_pos->tran.y - last_last_elbow_pos->tran.y;
    dz = last_elbow_pos->tran.z - last_last_elbow_pos->tran.z;
    double last_elbow_vel = sqrt(dx * dx + dy * dy + dz * dz) / input_status->trajCycleTime;

    double elbow_vel_limit = safety_info.safety_config.elbow_vel_limit_reduce;

    elbow_vel_limit += LINEAR_SPEED_THRESHOLD;
    //is decelerating?
    int overspeed = check_linear_vel_limit(elbow_vel, elbow_vel_limit);

    if (overspeed && elbow_vel > last_elbow_vel)
    {
        rtapi_print("[safety_check_elbow_vel_limit] : elbow_vel = %f, elbow_vel_limit = %f, last_elbow_vel = %f\n", elbow_vel, elbow_vel_limit, last_elbow_vel);
        return 1;
    }
    return 0;
}

/*
judge stop time, use maxacc to check, return 1 outof limit
需要判断机器人是否处于缩减模式,根据模式取约束值
TODO : 增加扭矩约束算法之后，加速度值实时变化，停止时间安全功能存在问题
TODO : 超出了安全STOP TIME，但是已经在减速，是否算violate？？？
待修改 : 应该使用反馈值计算 -- 区分关节运动和笛卡尔运动
*/
int safety_check_stop_time_limit(safety_double_check_inputp_t* input_status)
{
    double safe_dec_time = safety_info.safety_config.stoppingTime;

    //is reduce mode? decelerating??
    int is_reduce_mode = input_status->reduce_mode;
    if (is_reduce_mode)
    {
        safe_dec_time = safety_info.safety_config.stopping_time_reduce;
    }

    safe_dec_time += TIME_THRESHOLD;
    if (input_status->dec_time > safe_dec_time)
    {
        return 1;
    }
    return 0;
}

/*
judge stop distance, use maxacc to check, return 1 outof limit
需要判断机器人是否处于缩减模式,根据模式取约束值
TODO : 增加扭矩约束算法之后，加速度值实时变化，停止distance安全功能存在问题
TODO : 超出了安全距离，但是已经在减速，是否算violate？？？
*/
int safety_check_stop_dist_limit(safety_double_check_inputp_t* input_status)
{
    // stop distance only for movl and movc (sphercial for non_ori linear blend)
    //    // int motion_type = tpManager_GetMotType(0);
    // if(motion_type != TC_LINEAR && motion_type != TC_MOVC && motion_type != TC_SPHERICAL)
    // {
    //     return 0;
    // }

    double safe_dec_dist = safety_info.safety_config.stoppingDist;
    //is reduce mode? decelerating??
    int is_reduce_mode = input_status->reduce_mode;
    if (is_reduce_mode)
    {
        safe_dec_dist = safety_info.safety_config.stopping_dist_reduce;
    }

    safe_dec_dist += DIST_THRESHOLD;
    if (input_status->dec_dist > safe_dec_dist)
    {
        return 1;
    }
    return 0;
}

/*
judge safe plane, return safe_plane_violation_t
停止、保护性停止、缩减模式,
允许从非安全区域返回安全区域
肘部约束是否勾选

TBD: 进入非安全区域后通过hal_ui触发响应模式，可能存在延时；

----------- 勾选“运行启动”时，不对直线/关节运动至该点生效 -----------
*/
safe_plane_violation_t safety_check_safe_plane(ZucPose* cur_cart_pos_to_world,
                                               ZucPose* last_cart_pos_to_world,
                                               ZucPose* cur_elbow_pos_to_world,
                                               ZucPose* last_elbow_pos_to_world,
                                               safety_double_check_inputp_t* input_status)
{
    safe_plane_violation_t safeplane_violate = {0};
    //
    double r_elbow = safety_info.safety_config.elbow_sphere_radius;
    int safezone_ret = recheck_safe_zone(
        safety_info.safety_config.zone, r_elbow, cur_cart_pos_to_world, cur_elbow_pos_to_world, last_cart_pos_to_world, last_elbow_pos_to_world);
    //safezone_ret非零时，机器人处于危险区域且远离安全区域
    //判断安全平面设置模式,根据安全模式与机器人当前状态判断是否违反安全功能
    //TODO: 拖拽也要做安全平面检测
    if (safezone_ret)
    {
        if (safety_info.safety_config.zone.safeZoneMode == 0 && input_status->not_trig_abort)
        {
            //停止模式：机器人处于危险区域且在远离安全区域，但控制器没有触发停止(motAbortMotion, tpAbort, zucmotDebug->enabling = 0)
            safeplane_violate.mode_when_violate = 0;
            safeplane_violate.safe_plane_violation = 1;
        }

        if (safety_info.safety_config.zone.safeZoneMode == 1 && input_status->motion_state == ZUCMOT_MOTION_COORD &&
            is_protec_mode_safeplane_violation(input_status))
        {
            //保护性停止模式：安全平面触发的保护性停止仅对联动模式生效
            //安全平面触发确认 : zucmotStatus->safezone_confirm == 1(protec stop mode, reduce mode)
            safeplane_violate.mode_when_violate = 1;
            safeplane_violate.safe_plane_violation = 1;
        }

        if (safety_info.safety_config.zone.safeZoneMode == 0 && !(input_status->reduce_mode))
        {
            //缩减模式:超出安全平面应触发缩减模式标志位,否则异常
            safeplane_violate.mode_when_violate = 2;
            safeplane_violate.safe_plane_violation = 1;
        }
    }

    return safeplane_violate;
}

/*
judge tool ori, return 1 
允许从非安全区域返回安全区域
检测工具姿态是否超出且远离限制时,是否motAbortMotion，tpAbort
该接口中不考虑安全距离
TODO: 后将圆锥轴线和被约束轴线变换到基座坐标系后，此处需要更改
*/
int safety_check_tool_ori(ZucPose* cur_cart_pos_to_world, ZucPose* last_cart_pos_to_world, ZucPose* cur_tool_offsets, int not_trig_abort)
{
    int ret = 0;

    ToolOriCons tool_ori_cons;
    tool_ori_cons.conical_tip_choice = safety_info.safety_config.tool_ori.conical_tip_choice;
    memcpy(tool_ori_cons.conical_cenrpy, safety_info.safety_config.tool_ori.conical_cenrpy, sizeof(double) * 3);
    tool_ori_cons.deviation = safety_info.safety_config.tool_ori.deviation;
    tool_ori_cons.deviation_warning = safety_info.safety_config.tool_ori.deviation_warning;
    tool_ori_cons.tool_properties_pan = safety_info.safety_config.tool_ori.tool_properties_pan;
    tool_ori_cons.tool_properties_tilt = safety_info.safety_config.tool_ori.tool_properties_tilt;

    double cur_incl_angle = 0.0, last_incl_angle = 0.0;
    int cur_overrange = check_tool_ori(cur_cart_pos_to_world, cur_tool_offsets, &tool_ori_cons, &cur_incl_angle);
    int last_overrange = check_tool_ori(last_cart_pos_to_world, cur_tool_offsets, &tool_ori_cons, &last_incl_angle);

    //判断是否已经触发tpabort()和motAbortMotion()
    // int not_trig_abort = is_tpabort_motabort_triggered();
    //若夹角仍然增大且没有触发abort，则行为异常
    if (cur_overrange && (cur_incl_angle > (last_incl_angle + 0.001)) && not_trig_abort)
    {
        ret = 1;
    }
    return ret;
}

//return : 0 : safe 	1:dangerous
int recheck_safe_zone(SafeZone zone,
                      double r_elbow,
                      ZucPose* cur_cart_pos_to_world,
                      ZucPose* cur_elbow_pos_to_world,
                      ZucPose* last_cart_pos_to_world,
                      ZucPose* last_elbow_pos_to_world)
{
    int ret = 0;

    int elbow_limit_flag = zone.elbow_pos_limit_enable;
    double delt_dist = 0.0;
    double delt_elbow_dist = 0.0;
    double dist[6] = {0.0};       //cur_cart_pos_to_world距离安全平面的距离，在安全区域时为正，在危险区域时为负
    double last_dist[6] = {0.0};  //last_cart_pos_to_world距离安全平面的距离，在安全区域时为正，在危险区域时为负
    double dist_elbow[6] = {0.0};
    double last_dist_elbow[6] = {0.0};
    double safezone_eps = -1e-8;

    // only for safe plane now
    for (int i = 0; i < 6; i++)
    {
        //是否启用该安全平面
        if (zone.zone.plane[i].enable)
        {
            int trig_stop_cur = check_safe_plane(zone.zone.plane[i], cur_cart_pos_to_world, &dist[i], 0, r_elbow);
            int trig_stop_elbow_cur = 0;
            int trig_stop_elbow_last = 0;
            int trig_stop_last = 0;

            if (elbow_limit_flag)
            {
                trig_stop_elbow_cur = check_safe_plane(zone.zone.plane[i], cur_elbow_pos_to_world, &dist_elbow[i], 1, r_elbow);
            }
            if (zone.safeZoneMode == 0)
            {
                trig_stop_last = check_safe_plane(zone.zone.plane[i], last_cart_pos_to_world, &last_dist[i], 0, r_elbow);
                trig_stop_elbow_last = check_safe_plane(zone.zone.plane[i], last_elbow_pos_to_world, &last_dist_elbow[i], 1, r_elbow);

                delt_dist = dist[i] - last_dist[i];
                trig_stop_cur = trig_stop_cur && (delt_dist < safezone_eps);  // out of safety zone and move away from safety zone
                if (elbow_limit_flag == 1)
                {
                    delt_elbow_dist = dist_elbow[i] - last_dist_elbow[i];
                    trig_stop_elbow_cur = trig_stop_elbow_cur && (delt_elbow_dist < safezone_eps);
                    last_dist_elbow[i] = dist_elbow[i];
                }
                ret = ret || trig_stop_cur || trig_stop_elbow_cur;
            }
            else
            {
                ret = ret || trig_stop_cur || trig_stop_elbow_cur;
            }
        }
    }
    return ret;
}

// 停止模式下的安全平面是否正常执行安全功能
// 停止模式：机器人处于危险区域且在远离安全区域，但控制器没有触发停止(motAbortMotion, tpAbort, zucmotDebug->enabling = 0)
int is_stop_mode_safeplane_violation()  //not used
{
    int ret = 0;
    // int teleop_tp_enable = 0, teleop_tool_tp_enable = 0, free_tp_enable = 0, tp_aborting = 0, debug_dragging = 0;
    // for(int axis_num = 0; axis_num < CART_DIM; axis_num ++)
    // {
    //     teleop_tp_enable = teleop_tp_enable || axes[axis_num].teleop_tp.enable;
    //     teleop_tool_tp_enable = teleop_tool_tp_enable || axes[axis_num].teleop_tool_tp.enable;
    // }

    // for(int jnum = 0; jnum < safety_info.numJoints; jnum ++)
    // {
    //     free_tp_enable = free_tp_enable || joints[jnum].free_tp.enable;
    // }

    //    // tp_aborting = tpManager_tpIsAbort();
    // debug_dragging = zucmotDebug->dragging;

    // ret = teleop_tp_enable || teleop_tool_tp_enable || free_tp_enable || tp_aborting || debug_dragging;
    ret = is_tpabort_motabort_triggered();
    return ret;
}

// 保护性停止模式下的安全平面是否正常执行安全功能,安全平面的保护性停止仅对coordinate模式生效
// 保护性停止模式：未退出“保护性停止”时，机器人应处于减速或停止状态:tp->pausing == 1;
int is_protec_mode_safeplane_violation(safety_double_check_inputp_t* input_status)
{
    int ret = 0;
    if (input_status->safezone_confirm == 0)  //确认退出保护性停止时zucmotStatus->safezone_confirm == 1，不再生效
    {
        ret = input_status->tp_is_pausing ? 0 : 1;
    }
    return ret;
}

int is_reduce_mode_safeplane_violation()  //not used now
{
    int ret = 0;
    ret = zucmotStatus->reduce_mode ? 0 : 1;
    return ret;
}

//判断是否触发tpabort、motionabort, 触发tpabort、motionabort后teleop_tp/teleop_tool_tp/free_tp以及debug_dragging等于零，aborting标志位为1
//return 0: already triggered
//return 1: not triggered
int is_tpabort_motabort_triggered()
{
    int ret = 0;

    int teleop_tp_enable = 0, teleop_tool_tp_enable = 0, free_tp_enable = 0, tp_aborting = 0, debug_dragging = 0;
    // for (int axis_num = 0; axis_num < CART_DIM; axis_num++)
    // {
    //     teleop_tp_enable = teleop_tp_enable || axes[axis_num].teleop_tp.enable;
    //     teleop_tool_tp_enable = teleop_tool_tp_enable || axes[axis_num].teleop_tool_tp.enable;
    // }

    // for (int jnum = 0; jnum < zucmotConfig->numJoints; jnum++) { free_tp_enable = free_tp_enable || joints[jnum].free_tp.enable; }

    //    tp_aborting = tpManager_tpIsAbort();
    debug_dragging = zucmotDebug->dragging;

    ret = teleop_tp_enable || teleop_tool_tp_enable || free_tp_enable || (!tp_aborting) || debug_dragging ? 1 : 0;
    return ret;
}

void recheck_set_maxjpos(double maxjpos, int index)
{
    safety_info.safety_config.max_pos_limit[index] = maxjpos;
    rtapi_print(" ==============>>>>>>>>>>>>>>> [safety_double_check] : max_pos_limit[%d] = %f\n", index, safety_info.safety_config.max_pos_limit[index]);
    return;
}

void recheck_set_minjpos(double minjpos, int index)
{
    safety_info.safety_config.min_pos_limit[index] = minjpos;
    rtapi_print(" ==============>>>>>>>>>>>>>>> [safety_double_check] : min_pos_limit[%d] = %f\n", index, safety_info.safety_config.min_pos_limit[index]);
    return;
}

void recheck_set_maxjvel(double maxjvel, int index)
{
    safety_info.safety_config.jvel_limit[index] = maxjvel;
    rtapi_print(" ==============>>>>>>>>>>>>>>> [safety_double_check] : jvel_limit[%d] = %f\n", index, safety_info.safety_config.jvel_limit[index]);
    return;
}

void recheck_set_maxtcpvel(double maxtcpvel)
{
    safety_info.safety_config.tcpVelLimit = maxtcpvel;
    rtapi_print(" ==============>>>>>>>>>>>>>>> [safety_double_check] : tcpVelLimit = %f\n", safety_info.safety_config.tcpVelLimit);
    return;
}

void recheck_set_stop_dist(double stop_dist)
{
    safety_info.safety_config.stoppingDist = stop_dist;
    rtapi_print(" ==============>>>>>>>>>>>>>>> [safety_double_check] : stoppingDist = %f\n", safety_info.safety_config.stoppingDist);
    return;
}

void recheck_set_stop_time(double stop_time)
{
    safety_info.safety_config.stoppingTime = stop_time;
    rtapi_print(" ==============>>>>>>>>>>>>>>> [safety_double_check] : stoppingTime = %f\n", safety_info.safety_config.stoppingTime);
    return;
}

void recheck_set_elbow_radius(double elbow_radius)
{
    safety_info.safety_config.elbow_sphere_radius = elbow_radius;
    rtapi_print(" ==============>>>>>>>>>>>>>>> [safety_double_check] : elbow_sphere_radius = %f\n", safety_info.safety_config.elbow_sphere_radius);
    return;
}

void recheck_set_maxelbowvel_reduce(double max_elbow_vel_reduce)
{
    safety_info.safety_config.elbow_vel_limit_reduce = max_elbow_vel_reduce;
    rtapi_print(" ==============>>>>>>>>>>>>>>> [safety_double_check] : elbow_vel_limit_reduce = %f\n", safety_info.safety_config.elbow_vel_limit_reduce);
    return;
}

void recheck_set_maxtcpvel_reduce(double maxtcpvel_reduce)
{
    safety_info.safety_config.tcp_vel_reduce = maxtcpvel_reduce;
    rtapi_print(" ==============>>>>>>>>>>>>>>> [safety_double_check] : tcp_vel_reduce = %f\n", safety_info.safety_config.tcp_vel_reduce);
    return;
}

void recheck_set_stop_dist_reduce(double stop_dist_reduce)
{
    safety_info.safety_config.stopping_dist_reduce = stop_dist_reduce;
    rtapi_print(" ==============>>>>>>>>>>>>>>> [safety_double_check] : stopping_dist_reduce = %f\n", safety_info.safety_config.stopping_dist_reduce);
    return;
}

void recheck_set_stop_time_reduce(double stop_time_reduce)
{
    safety_info.safety_config.stopping_time_reduce = stop_time_reduce;
    rtapi_print(" ==============>>>>>>>>>>>>>>> [safety_double_check] : stopping_time_reduce = %f\n", safety_info.safety_config.stopping_time_reduce);
    return;
}

void recheck_set_safe_zone(SafeZone safe_zone)
{
    safety_info.safety_config.zone = safe_zone;
    rtapi_print(" ==============>>>>>>>>>>>>>>> [safety_double_check] : openEnable = %d, autoEnable = %d, elbow_limit_enable = %d, wrist_limit_enable = %d, "
                "safeZoneMode = %d\n",
                safety_info.safety_config.zone.openEnable,
                safety_info.safety_config.zone.autoEnable,
                safety_info.safety_config.zone.elbow_pos_limit_enable,
                safety_info.safety_config.zone.wrist_pos_limit_enable,
                safety_info.safety_config.zone.safeZoneMode);
    return;
}

void recheck_set_tool_ori(zuc_set_attitude_safety tool_ori)
{
    safety_info.safety_config.tool_ori = tool_ori;
    rtapi_print(" ==============>>>>>>>>>>>>>>> [safety_double_check] : openEnable = %d, autoEnable = %d, conical_tip_choice = %d, tilt = %f, pan = %f, "
                "deviation = %f, deviation_warning = %f, conical_cenrpy = %f %f %f\n",
                safety_info.safety_config.tool_ori.enableLimAttitudeSafetyOpen,
                safety_info.safety_config.tool_ori.enableLimAttitudeSafetyAuto,
                safety_info.safety_config.tool_ori.conical_tip_choice,
                safety_info.safety_config.tool_ori.tool_properties_tilt,
                safety_info.safety_config.tool_ori.tool_properties_pan,
                safety_info.safety_config.tool_ori.deviation,
                safety_info.safety_config.tool_ori.deviation_warning,
                safety_info.safety_config.tool_ori.conical_cenrpy[0],
                safety_info.safety_config.tool_ori.conical_cenrpy[1],
                safety_info.safety_config.tool_ori.conical_cenrpy[2]);
    return;
}

void recheck_set_joints_num(int joints_num)
{
    safety_info.numJoints = joints_num;
    rtapi_print(" ==============>>>>>>>>>>>>>>> [safety_double_check] : numJoints = %d\n", safety_info.numJoints);
    return;
}